// 该例程需要配合sub_broadcaster功能包来完成，两个小海龟分别动态发布TF，
// 利用该例程来监听两个小海龟的TF来达到让第二只小海龟跟随第一只小海龟移动的目的。


#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "geometry_msgs/msg/twist.h"
#include "rclcpp/rclcpp.hpp"
#include "tf2/exceptions.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "turtlesim/srv/spawn.hpp"

using namespace std::chrono_literals;


class listener_broadcaster :public rclcpp::Node
{
private:
    rclcpp::Client<turtlesim::srv::Spawn>::SharedPtr cli_{nullptr};
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_;
    bool turtle_spawning_service_ready_;
    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
    void timer_cb()
    {
        if(turtle_spawning_service_ready_){
            geometry_msgs::msg::TransformStamped t;
            t = tf_buffer_->lookupTransform("turtle2","turtle1",tf2::TimePointZero);
            
            geometry_msgs::msg::Twist msg;
            static const double scaleRotationRate = 1.0;
            msg.angular.z = scaleRotationRate * atan2(
                t.transform.translation.y,
                t.transform.translation.x);
            
            static const double scaleForwardSpeed = 0.5;
            msg.linear.x = scaleForwardSpeed * sqrt(
                pow(t.transform.translation.x,2)+
                pow(t.transform.translation.y,2));

            pub_->publish(msg);
        }
        else{
            if(cli_->service_is_ready()){
                auto request =std::make_shared<turtlesim::srv::Spawn::Request>();
                request->x = 4.0;
                request->y = 2.0;
                request->theta = 0.0;
                request->name = "turtle2";

                using ServiceResponseFuture = rclcpp::Client<turtlesim::srv::Spawn>::SharedFuture;
                auto response_received_callback = [this](ServiceResponseFuture future){
                    auto result = future.get();
                    if(strcmp(result->name.c_str(),"turtle2")==0){
                        turtle_spawning_service_ready_ = true;
                    }
                };
                auto result = cli_->async_send_request(request,response_received_callback);
            }
        }
    }
public:
    listener_broadcaster(/* args */);
    ~listener_broadcaster();
};

listener_broadcaster::listener_broadcaster(/* args */) :Node("listener_broadcaster_node"),turtle_spawning_service_ready_(false)
{
    cli_ = this->create_client<turtlesim::srv::Spawn>("spawn");
    timer_ = this->create_wall_timer(500ms,std::bind(&listener_broadcaster::timer_cb,this));
    pub_ =this->create_publisher<geometry_msgs::msg::Twist>("turtle2/cmd_vel", 1);
}

listener_broadcaster::~listener_broadcaster()
{
}


int main(int argc, char const *argv[])
{
    rclcpp::init(argc,argv);
    auto node = std::make_shared<listener_broadcaster>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
